# coding=UTF-8

import numpy as np
import cv2



def triangulateOnePoint(track, P_dict):
    """
    功能：通过三角测量，计算一个点轨迹的3D空间点
          使用方法参见《MVG》的P312
    """
    M=len(track.idx_list)
    A=np.zeros((M*2,4))
    for i in range(M):
        idx=track.idx_list[i]
        pt2D=np.array(track.pos_list[i])
        P=P_dict[idx]
        A[2*i,:]=pt2D[0]*P[3-1,:]-P[1-1,:]
        A[2*i+1,:]=pt2D[1]*P[3-1,:]-P[2-1,:]

    U,S,Vt=np.linalg.svd(A)
    X=Vt[-1,:]      ##取Vt的最后一行，即V的最后一列
    X=X/X[-1]

    return X[:3]    #返回X,Y,Z

def triangulateMultiview(tracks,camPoses,K):
    """
    通过三角测量，计算给定点轨迹序列的3D空间点
    输入：
        tracks-->PointTrack对象列表
        camPoses-->各相机相对于参考坐标系的朝向和位置
    输出：
        生成的3D空间点，Nx3的np.array类型矩阵
    """
    pts3D=[]
    ###提取出相机矩阵P
    ###注意：要将相机在参考坐标系中的方位转换成参考坐标系在相机中的方位
    P_dict={}
    for id,orient,loc in camPoses:
        R=orient.transpose()
        t=-np.dot(R,loc)
        P=np.dot(K, np.hstack((R,t)) )
        P_dict[id]=P
    for track in tracks:
        pt3D=triangulateOnePoint(track,P_dict)
        pts3D.append(pt3D)
    return np.array(pts3D)